perm filename ARMSW.NEW[SYS,HE] blob
sn#064717 filedate 1973-10-01 generic text, type T, neo UTF8
COMMENT ⊗ VALID 00017 PAGES
RECORD PAGE DESCRIPTION
00001 00001
00003 00002 IFN USER+TRACK,<0>
00006 00003 NXTJIF:
00008 00004 WATSTL: CONI CLOCK,AC
00010 00005 MAIN: TRNE RUN
00013 00006 TT: PUSHJ P,REFLEX
00014 00007 ANGLES: MOVEI I,5
00017 00008 IFE USER,<
00018 00009 SERVO: TDNN DATWD,BMASK(I) IS THIS JOINT RUNNING?
00020 00010 DRIVE:
00023 00011 IFN SIMU,< IF IN SIMU PREDICT NEXT VALUE OF THETA
00025 00012 NXTH: MOVE AC,[XWD T0,T0P]
00028 00013 EVAL: MOVEI I,5
00033 00014 DHO: AOJ I, DISASTER HAS OCCURED
00036 00015 NEXT: TRNE RUN
00040 00016 PREAD:
00042 00017 NON: POINT 12,INNER(AC),11
00047 ENDMK
⊗;
IFN USER+TRACK,<0>
STRADR: MOVE BITSAV ;Enters here every 1/60 th. seccond
IFE USER+TRACK,<
TRNE DTST
CALLI 400024
DATAO ARM,[2]
SOSLE STWT
CALLI 400024
>
MOVE P,[XWD -PDLL,PDL] ;PUSH DOWN STACK
MOVE CBUF,CBUFS
MOVE DATWD,DATWDS
IORI DATWD,2 ;YELLOW ARM
IFN REPORT+TRACK,<
MOVE DATA,DATPT
>
IFN USER+TRACK,<
TRNE DTST
JRST SWDONE
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST NCHK
>
CONSZ 40 ;CHECK TO SEE IF ON PDP-6
JRST[ MOVEI TAC,PDPERR
MOVEM TAC,TRAJER
JRST TOFF]
CONI 0,AC
ANDI AC,7
CONO 0,2050(AC)
>
TRZE TRIP ;TURN ON ARM
JRST[
DATAO ARM,[3]
NCHK: MOVEI K,51B23 ;-10 VOLT REFERENCE
PUSHJ P,PREAD ;REFTR← -10 volt reference
LDB AC,SNUM ;PICK UP MIDDLE SAMPLE
ANDI DACVAL,7777 ;LEAVE LAST SAMPLE IN J
ADDI AC,(DACVAL) ;ADD THEM TOGETHER
CAIGE AC,17000 ;IS THE FUSE BLOWN
JRST[ MOVEI TAC,REFERR
MOVEM TAC,TRAJER
JRST TOFF]
MOVEM AC,REFTR ;REFERENCE READING
MOVEI I,2 ;FIRST THREE TACHS
MOVE K,VCHAN(I)
ZVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
MOVEM AC,VZ(I)
SOJGE I,ZVEL
JRST .+1]
IFN TRACK,<
SKIPE WALK
JRST TOK
>
MOVEI AC,=16666
MOVEM AC,ETIME
MOVSI AC,(1.0)
MOVEM AC,TDF
IFE USER,<
CONI CLOCK,AC
TLZ AC,777774
JRST STIME ;TIME IN MICRO SECONDS
>
IFN USER,<JRST .+2>
NXTJIF:
IFN USER,<JRST SWDONE>
IFN DEB<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
MOVE 17,ACSAV+17
>
IFN TRACK,<
SKIPE WALK
JRST TOK
>
IFN REPORT+TRACK,<
HRRZI TAC,(DATA)
CAIL TAC,DATO+DBL-1
JRST[ SUB DATA,[XWD DBL,DBL]
MOVE TAC,[XWD DATO+DBL,DATO]
BLT TAC,(DATA)
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
JRST RDP]
HRRZI AC,(DATA)
HRRZ TAC,DATIWD
SUBI AC,(TAC)
HRRZ MQ,DATPT
SUBI TAC,(MQ)
IMUL AC,TAC
RDP: SKIPLE AC
IFN TRACK,<PUSH DATA,[0]>
IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
MOVEI AC,DATO-1
HRRZ TAC,DATIWD
CAIL TAC,DATO+DBL-1
HRRM AC,DATIWD
MOVEM DATA,DATPT
>
IFE USER,<
MOVEI TAC,=2000
WATSTL: CONI CLOCK,AC
TLZ AC,777774
SUB AC,LTIME
JUMPGE AC,.+2
ADD AC,[=1000000]
IFN REPORT,<
CAIN TAC,=2000
JRST[ PUSH DATA,[<SIXBIT/TICK/>+1]
PUSH DATA,AC
JRST .+1]
>
MOVEM AC,ETIME
TRNE RUN
JRST[ MOVE MQ,AC
FSC MQ,233
FMPR MQ,[0.00006]
MOVEM MQ,TDF
MOVE AC,LTIME
ADD AC,ETIME
JRST SSTIME]
CAIGE AC,=16667 ;delta T greater than 16.7 msec.?
SOJGE TAC,WATSTL
JUMPL TAC,TE
MOVSI AC,(1.0)
MOVEM AC,TDF
MOVE AC,LTIME
ADDI AC,=16667
SSTIME: TLNE AC,777774
SUB AC,[=1000000]
STIME: MOVEM AC,LTIME ;LTIME←LTIME+16.7 msec.
>
SOSGE COUNT
JRST[TE:MOVEI AC,TERR
MOVEM AC,TRAJER
JRST ZITO]
MAIN: TRNE RUN
JRST[ PUSHJ P,EVAL
PUSHJ P,NXTH
JRST .+1]
MOVEI J,6
PUSHJ P,ANGLES ;read joint angles
TRNE RONLY ;read angles only?
JRST[ PUSHJ P,HEAD ;read hand also
MOVE AC,HAND
MOVEM AC,SET
PUSHJ P,REFLEX
PUSHJ P,READED] ;DOES NOT RETURN
TRNN RUN
JRST[ SKIPG TAC,FUNCT ;not run, mabye a function
PUSHJ P,SETSET
JUMPL TAC,NEXT
JUMPE TAC,[CAIL CBUF,BUF;try next buffer
SETOM (CBUF) ;dont do this if first buffer
MOVEM CBUF,CBUFS
ADDI CBUF,1001
CAILE CBUF,BUF2
MOVEI CBUF,BUF ;Advance buffer
MOVE 2,(CBUF)
JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
JRST SWDONE]
JUMPN 2,BUFRDY ;the last buffer stop.
JRST[ TRO DTST
MOVEI 2,WAITIN
HRRM 2,JUMPBK
MOVEM 2,STKPTR
IFN REPORT,<PUSH DATA,[0]>
JRST BUFWT]
BUFRDY: HRRZ 2,(CBUF)
ADDI 2,-1(CBUF)
HRRZM 2,STKPTR
JRST NEXT]
CAIN TAC,6
JRST SETCH
JRST TT] ;otherwise perform function
JUMPE J,[RUDONE:
TRNN NSET
PUSHJ P,SETSET
TRZ DROP+RUN+FINAL+WOB ;if J is zero then end of run
TDZ DATWD,[770001252500]
TRZE STP
JRST[ MOVEI TAC,STPERR
MOVEM TAC,TRAJER
POP P,TAC
JRST TOFF]
SKIPN AC,FUNCT
JRST NEXT
CAIN AC,22
JRST[ TRZ HCL
SETZM TDAT
SETZM FUNCT
DATAO DEVT,TDAT
JRST NEXT]
MOVEI AC,=16667
MOVEM AC,ETIME
JRST .+1]
TT: PUSHJ P,REFLEX
DISP: MOVE AC,FUNCT
JUMPL AC,.+2
CAILE AC,MAXFN
JRST[ MOVEI TAC,FUNERR
MOVEM TAC,TRAJER
JRST TOFF]
JRST @.+1(AC) ;Perform function
NXTJIF ;0
HOPEN ;1
HCLOSE ;2
NXTJIF ;3
PLACE ;4
NUDGE ;5
SETCH ;6
STOP ;7
SAVE ;10
RESTORE ;11
CENTER ;12
SET.ARM ;13 SET_ARM
WOBBLE ;14
SEARCH ;15
AOJI ;16
SLAVE ;17
HOME ;20
PATH ;21
TOOL ;22
XLIST
SUBTTL SPACEWAR CODE ANGLE MEASUREMENT LOU PAUL
LIST
ANGLES: MOVEI I,5
JFCL 10,.+1
MOVEI L,10
SETCHA: MOVEI K,CHA
TRNE ALT6
MOVEI K,N6CHA
SKIPA
RSTRT: MOVE K,CHAN(I)
SOJL L,REDER
IFE USER,<
PUSHJ P,PREAD
LDB AC,SNUM
CAIN I,5
JRST[ TRNE POTREAD
JRST .+1
CAIL AC,1000
CAILE AC,7000
JRST[ TRC ALT6
JRST SETCHA]
MOVEI K,CHA5
JRST .+1]
ANDI DACVAL,7777
ADDI AC,(DACVAL)
SETZ TAC,
DIV AC,REFTR
ASH AC,-26
MOVE TAC1,AC
TRNE POTREAD
JRST[ ADDI TAC1,=1000
JRST EXON]
ASHC AC,-11 ;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
LDB MQ,NON(I) ;GET LOWER DIFF
ADDI TAC1,(MQ) ;ADD IT TO READING
AOJ AC,
LDB AC,NON(I) ;GET UPPER DIFFERENCE
SUBI AC,(MQ)
MUL TAC,AC ;INTERPOLATE
ADD TAC1,TAC ;CORRECTED READING
FSC TAC1,226 ;FLOAT
FMPR TAC1,SCALE(I)
FADR TAC1,OFF(I)
>
IFN USER,<MOVE TAC1,TH(I)>
CAIN I,5
JRST[ TRNE ALT6
FADR TAC1,OFF6
FADR TAC1,ROT6
MOVEM TAC1,TH+5
FSBR TAC1,T0+5
MOVSI MQ,(180.0)
CAML TAC1,MQ
JRST[ INCROT: MOVN MQ,MQ
FSC MQ,1
FADRM MQ,ROT6
FADRM MQ,TH+5
JRST THS]
MOVN MQ,MQ
CAMG TAC1,MQ
JRST INCROT
JRST THS]
EXON: MOVEM TAC1,TH(I) ;THETA
THS:
IFN REPORT,<
MOVE MQ,TH(I)
FSBR MQ,T0(I)
MOVEM MQ,JER(I)
>
IFE USER,<CAILE I,2>
JRST[ MOVE AC,TH(I)
EXCH AC,THP(I)
FSBR AC,THP(I)
FDVR AC,TDF
MOVNM AC,TD(I)
JRST SERVO]
IFE USER,<
MOVE K,VCHAN(I)
NVEL: PUSHJ P,PREAD
LDB AC,SNUM
ANDI DACVAL,7777
ADDI AC,(DACVAL)
SUB AC,VZ(I)
FSC AC,216
FMPR AC,VSCALE(I)
MOVEM AC,TD(I)
>
XLIST
SUBTTL SPACEWAR CODE SERVO CALC. LOU PAUL
LIST
SERVO: TDNN DATWD,BMASK(I) ;IS THIS JOINT RUNNING?
SOJA J,NXTJT ;NO TRY NEXT JOINT
MOVE MQ,FBI
TDNN MQ,GMASK(I) ;FREE JOINT?
SOJA J,DRIVE ;YES. DONT APPLY FEEDBACK
MOVE TAC,TH(I)
FSBR TAC,T0(I)
MOVEM TAC,ET0(I) ;POSITION ERROR
TRNE FINAL
JRST[ TLNN DATWD,NNUL ;DONT NULL ERRORS
TRNE WOB ;OR WOBBLE
SOJA J,.+1 ;JUST STOP
FADRM TAC,ERRINT(I) ;INTEGRATE ERROR
MOVM MQ,TAC ;|ERROR|
CAMLE MQ,ERR(I) ;LESS THAN TOLERANCE?
JRST .+1 ;NO KEEP GOING
MOVM MQ,TD(I) ;|VEL|
CAMLE MQ,ERR(I) ;LESS THAN ERROR TOLERANCE?
JRST .+1 ;NO KEEP ON
SO: TDZ DATWD,BMASK(I)
DATAO ARM,DATWD ;PUT ON THE BRAKES
HRLZ MQ,I
TRO MQ,400000
DATAO WIDTH,MQ ;SET DRIVE TO ZERO
SOJA J,NXTJT]
FMPR TAC,KE(I) ;KE * POSITION ERROR
MOVN AC,ERRINT(I)
FMPR AC,KI(I) ;-KI * INTEGRAL ERROR
FSBR AC,TAC ;-KE * POS ERR -KI * INT ERR
MOVEM AC,TORE(I) ;ERROR CORRECTING TORQUE
MOVN TAC,TD(I) ;VELOCITY
FADR TAC,TDP(I) ;-VEL ERROR
FMPR TAC,KV(I) ;-KV * VEL ERROR
FADR AC,TAC
FADRB AC,T(I) ;PRELOADER WITH T(G) AND J*TDD(I)
XLIST
SUBTTL SPACEWAR CODE DRIVE CALC. LOU PAUL
LIST
DRIVE:
MOVE MQ,TD(I)
JUMPN MQ,.+2
MOVN MQ,ET0(I)
FMPR MQ,T(I)
JUMPL MQ,[ MOVM MQ,T(I)
FSBR MQ,F0(I)
JUMPL MQ,[ FDVR MQ,F0(I)
FMPR MQ,V0(I)
FSC MQ,1
FADR MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
JRST CEMF]
MOVSI AC,(1.0)
FSBR AC,PK(I)
FMPR MQ,AC
FMPR MQ,KM(I)
FADR MQ,V0(I)
SKIPG T(I)
MOVN MQ,MQ
JRST CEMF]
SKIPN T(I)
JRST[ SKIPN TD(I)
JRST CEMF
MOVE MQ,V0(I)
SKIPGE TD(I)
MOVN MQ,MQ
JRST CEMF]
JUMPE MQ,NOEX
MOVE MQ,V0(I)
SKIPGE T(I)
MOVN MQ,MQ
NOEX: MOVSI AC,(1.0)
FADR AC,PK(I)
FMPR AC,T(I)
FMPR AC,KM(I)
FADR MQ,AC
CEMF:
MOVE AC,EMF(I)
FMPR AC,TD(I)
MOVSI TAC1,(30.0)
JUMPGE MQ,DRVLT
MOVN AC,AC
DRVLT: FSBR TAC1,AC ;Available drive voltage
CAMGE TAC1,[1.0]
JRST MAXDRV
VELOK: MOVM AC,MQ
FDVR AC,TAC1 ;AC= REL TIME ON
FIX AC,211000 ;1=16
CAML AC,[4000000]
JRST DHO ;too much force stop the arm
CAILE AC,776000
MAXDRV: MOVEI AC,776000
IFN REPORT+TRACK+USER+DEB,<
MOVEM AC,MOTOR(I)
SKIPGE MQ
MOVNS MOTOR(I)
>
TRC AC,400000
HRL AC,I
SETDRV:
IFN TRACK,<SKIPN WALK>
DATAO WIDTH,AC
TDZ DATWD,DMASK(I)
JUMPGE MQ,ISP
TDO DATWD,DMASK(I)
ISP: SKIPE REV(I)
TDC DATWD,DMASK(I)
DATAO ARM,DATWD
NXTJT: SOJGE I,RSTRT
JFCL 10,DHO
IFN TRACK,<SKIPN WALK>
IFN SIMU,< ;IF IN SIMU PREDICT NEXT VALUE OF THETA
MOVEI I,5
PNT: MOVE AC,T0(I) ;PRESENT VALUE
FSC AC,1 ;X 2
FSBR AC,T0P(I) ;PREVIOUS VALUE
MOVEM AC,TH(I) ;IS NEXT VALUE
SOJGE I,PNT
>
IFN REPORT,< ;OUTPUT ERRORS AND DRIVES
HRLI AC,PBLK
HRRI AC,1(DATA)
ADD DATA,[XWD 16,16]
BLT AC,(DATA)
>
SETZM ASTP
TRNE STP
JRST[ MOVEI I,5
MTL: MOVE MQ,TORE(I)
FDVR MQ,F0(I)
SKIPN TD(I)
FSC MQ,-2
FMPR MQ,STQ(I)
FADRM MQ,ASTP
SOJGE I,MTL
MOVE AC,ASTP
CAML AC,SDTQ
JRST[ AOS AC,ASTPP
CAIL AC,2
JRST[ TRZ STP
SUB P,[XWD 1,1]
SETZM ERRINT
MOVE AC,[XWD ERRINT, ERRINT+1]
BLT AC,ERRINT+5
JRST RUDONE]
POPJ P,]
SETZM ASTPP
POPJ P,]
POPJ P,
XLIST
SUBTTL SPACEWAR CODE LOU PAUL
LIST
NXTH: MOVE AC,[XWD T0,T0P]
BLT AC,T0P+5
TRNE WOB
JRST[ MOVEI I,2
WL1: SOSGE J,WOBCNT(I)
JRST[ MOVEI J,=19
MOVEM J,WOBCNT(I)
JRST WL2]
WL2: MOVE AC,SIN(J)
FMPR AC,WOBMAG
FADRM AC,TFF+3(I)
SOJGE I,WL1
JRST .+1]
TRNE DROP
JRST[ MOVEI I,5
OL: MOVE AC,DELTH(I)
FMPR AC,TDF
FADRM AC,DTH(I)
SOJGE I,OL
JRST JALS]
NTH:
TRNE INCREM
JRST[ MOVE MQ,ETIME
ADDB MQ,TICKS
CAML MQ,NTICKS
JRST[ TRZ INCREM
SKIPN TP
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
SKIPN TP(I)
TLNN DATWD,INT
JRST FIL1
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
FIL1: MOVEI I,5
FIL: MOVE AC,DELTH(I)
FADRB AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SETZM TDP(I)
SETZM DELTH(I)
SOJGE I,FIL
JRST COMT]
SETZ I,
DIV MQ,NTICKS
ASH MQ,-10
FSC MQ,200
MOVSI AC,(6.0)
FMPR AC,MQ
FADR AC,[-15.0]
FMPR AC,MQ
FADR AC,[10.0]
FMPR AC,MQ
FMPR MQ,MQ
FMPR MQ,AC
MOVEI I,5
LDC: MOVE AC,DELTH(I)
FMPR AC,MQ
FADR AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SKIPN TP
TLNN DATWD,INT
JRST LDC1
MOVE AC,DCI(I)
FMPR AC,MQ
FADR AC,PC(I)
MOVEM AC,CI(I)
LDC1: SOJGE I,LDC
JRST COMT]
JALS: MOVEI I,5
JAL: MOVE AC,DTH(I)
FADR AC,TFF(I)
MOVEM AC,T0(I)
SOJGE I,JAL
COMT: MOVE AC,[XWD CI,T]
BLT AC,T+5
TRNE FINAL
POPJ P,
MOVEI I,5
CTV: MOVE TAC1,T0(I)
FSBR TAC1,T0P(I)
FDVR TAC1,TDF
MOVE AC,TAC1
FSBR AC,TDP(I)
MOVEM TAC1,TDP(I)
FDVR AC,TDF
FMPR AC,MOTARM(I)
FADRM AC,T(I)
SOJGE I,CTV
POPJ P,
EVAL: MOVEI I,5
EL: MOVE J,TP(I) ;POINTS TO KOE5
JUMPE J,NE
MOVE MQ,ETIME
ADDB MQ,TJ(I)
CAML MQ,TN(I) ;NUMBER OF TICKS THIS SEGMENT
JRST[ ;END OF SEGMENT
CAIN I,5
JRST[ ;JOINT 6 HAS CONTROL WORD
MOVE TAC,TICKS
CAMGE TAC,NTICKS
TRO INCREM
LDB TAC,[POINT 3,1(J),2] ;LEVEL
JUMPN TAC,[ ;SWITCH POINT
SKIPG LOOP(TAC)
JRST[ ;INITIALIZE LOOP
LDB AC,[POINT 15,1(J),17]
MOVEM AC,LOOP(TAC) ;COUNT
JRST SETALT]
SOSLE LOOP(TAC) ;IN LOOP
SETALT: TRO ALT
JRST GETNXT]
JRST GETNXT]
GETNXT: TRNE ALT
JRST[ LDB J,[POINT 9,(J),8] ;ALT POINTER
JRST GOT]
LDB J,[POINT 9,(J),17] ;NEXT POINTER
JUMPE J,[SETZM TP(I)
TRO FINAL
MOVEI AC,=600
MOVEM AC,COUNT
MOVE TAC,[XWD NC,CI]
BLT TAC,CI+=11
MOVE AC,A4(I)
ADD AC,A3(I)
ADD AC,A2(I)
ADD AC,A1(I)
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
JRST NE]
GOT: ADDI J,(CBUF)
MOVEM J,TP(I)
SUB MQ,TN(I) ;MICRO SEC INTO NEW SEGMENT
MOVEM MQ,TJ(I) ;INITIALIZE
HRRZ AC,(J)
ASH AC,16
MOVEM AC,TN(I)
CAIN I,5
JRST[
HRRZ AC,1(J)
ADDI AC,(CBUF)
MOVEI MQ,6(AC)
MOVEI K,5
MOVE MQ,(MQ) ;NEW DATWD
FTF: MOVE TAC,GMASK(K)
TDNN TAC,FBI
JRST[ TDNN TAC,MQ ;WAS FREE
JRST FNF ;STILL FREE
MOVE TAC,TH(K)
MOVEM TAC,T0(K)
FSBR TAC,TFF(K)
MOVEM TAC,DTH(K)
MOVNM TAC,DELTH(K)
MOVE TAC,TN+5
ASH TAC,16
MOVEM TAC,NTICKS
SETZM TICKS
TRO INCREM
JRST FNF]
FNF: SOJGE K,FTF
PUSHJ P,UPDC
JRST DDELC]
DDELC: HLRE AC,-3(J)
MOVEM AC,A4(I)
HRRE AC,-3(J)
MOVEM AC,A3(I)
HLRE AC,-2(J)
MOVEM AC,A2(I)
HRRE AC,-2(J)
MOVEM AC,A1(I)
HLLZ AC,-1(J)
MOVEM AC,SC(I)
HRRE AC,-1(J)
MOVEM AC,A0(I)
MOVE MQ,TJ(I)
JRST ELL]
ELL: DIV MQ,TN(I)
TLNE DATWD,INT
CAIE I,5
JRST ELL1
MOVEI K,5
HLRE TAC,MQ
FSC TAC,211
DECC: MOVE AC,DCI(K)
FMPR AC,TAC
FADR AC,PC(K)
MOVEM AC,CI(K)
SOJGE K,DECC
ELL1: MOVE AC,A4(I)
MUL AC,MQ
ADD AC,A3(I)
MUL AC,MQ
ADD AC,A2(I)
MUL AC,MQ
ADD AC,A1(I)
MUL AC,MQ
ADD AC,A0(I)
XOR AC,SC(I)
FSC AC,0
MOVEM AC,TFF(I)
CAIN I,5
JRST[ MOVE AC,TFF+5
FSBR AC,T0+5
MOVSI MQ,(180.0)
CAML AC,MQ
JRST[ INCT0: FSC MQ,1
FADRM MQ,T0+5
JRST .+1]
MOVN MQ,MQ
CAMG AC,MQ
JRST INCT0
JRST .+1]
NE: SOJGE I,EL
TRZ ALT
POPJ P,
DHO: AOJ I, ;DISASTER HAS OCCURED
LSH I,3 ;JOINT NUMBER
IORI I,EXER
MOVEM I,TRAJER
POP P,I
ZITO: SETZM ERRINT
MOVE AC,[XWD ERRINT,ERRINT+1]
BLT AC,ERRINT+5
TOFF: MOVE AC,STKPTR
MOVEM AC,ERRSTK
DATAO DEVT,[0]
MOVEI L,1000
TRNE RONLY
JRST READED
TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB ;STOP THE ARM
SETZM TP
MOVE AC,[XWD TP,TP+1]
BLT AC,TP+5
PUSHJ P,SETSET
MOVNXT: AOS AC,STKPTR
MOVNX: SOJL L,[ LE:
MOVEI TAC,LOOPER
MOVEM TAC,TRAJER
JRST PW]
HLRE TAC,@STKPTR
CAIN TAC,3 ;IS IT A WAIT
JRST MOVNXT
CAIN TAC,SKPERR
JRST[ HRRZ TAC,(AC)
CAMN TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,(AC)
CAME TAC,TRAJER
JRST[ AOS STKPTR
JRST MOVNXT]
JRST NEXT]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,(AC),35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,(AC)
ANDI TAC,777770
TDNE TAC,TRAJER
JRST[ AOS STKPTR
JRST NEXT]
JRST MOVNXT]
JRST MOVNXT]
CAIN TAC,JMPREL
JRST[ HRRE TAC,(AC)
ADDI AC,(TAC)
HRRM AC,STKPTR
JRST MOVNX]
PW: CAIL AC,BUF
HRRM AC,JUMPBK
MOVEI AC,WAITIN
MOVEM AC,STKPTR
MOVEI AC,3
MOVEM AC,FUNCT
IFN TRACK,<SKIPN WALK>
DATAO ARM,[2] ;STOP THE ARM
IFN REPORT,<PUSH DATA,[0]>
READED: TRO DTST
IFN DEB,<
MOVEM 17,ACSAV+17
MOVEI 17,ACSAV
BLT 17,ACSAV+16
>
JRST SWDONE
NEXT: TRNE RUN
JRST[ SETZM FUNCT
JRST NXTJIF]
MOVEI L,1000
MOVE AC,FUNCT
CAIE AC,6 ;SET TOUCH
JRST[ MOVE TAC,SOTCHS
MOVEM TAC,SOTCH
TRZ STCH
JUMPE TAC,.+1
TRO STCH
JRST .+1]
IFN TRACK,<SKIPN WALK>
MOVEI DATWD,2
DATAO ARM,DATWD ;STOP THE ARM
INCPIC: AOS AC,STKPTR
PICKUP: SOJL L,LE
HLRE TAC,@STKPTR
CAIN TAC,JMPABS
JRST[JMPA:HRRZ TAC,@STKPTR
MOVEM TAC,STKPTR
HLRZ TAC,@STKPTR
CAIN TAC,3
JRST INCPIC
JRST PICKUP]
CAIN TAC,SKPERR
JRST[ HRRZ TAC,@STKPTR
CAMN TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPNER
JRST[ HRRZ TAC,@STKPTR
CAME TAC,TRAJER
AOS STKPTR
JRST INCPIC]
CAIN TAC,SKPSER
JRST[ LDB MQ,[POINT 3,@STKPTR,35]
LDB TAC,[POINT 3,TRAJER,35]
CAIN TAC,(MQ)
JRST[ HRRZ TAC,@STKPTR
ANDI TAC,777770
TDNE TAC,TRAJER
AOS STKPTR
JRST INCPIC]
JRST INCPIC]
CAIN TAC,JMPREL
JRST[ HRRE TAC,@STKPTR
ADD TAC,STKPTR
HRRM TAC,STKPTR
JRST PICKUP]
SETZM TRAJER
MOVEI I,100
MOVEM I,COUNT
TRZE TAC,MERGE
TRO BOTH
MOVEM TAC,FUNCT
CAIN TAC,3 ;IS IT A WAIT
JRST[ MOVE TAC,STKPTR
CAIL TAC,BUF
HRRM TAC,JUMPBK
MOVEI TAC,WAITIN
MOVEM TAC,STKPTR
IFN REPORT,<PUSH DATA,[0]>
TRO DTST
PUSHJ P,SWDONE]
MARK:
IFN REPORT,<
PUSH DATA,[<SIXBIT/NEXT/>+1]
MOVE TAC,FUNCT
HRL TAC,0
PUSH DATA,TAC
>
; JRST DISP
SWDONE:
MOVEM BITSAV
MOVEM DATWD,DATWDS
MOVEM CBUF,CBUFS
IFN REPORT+TRACK,<
MOVEM DATA,DATPT
>
IFE USER,<
IFN TRACK,<
SKIPE WALK
JRST @STRADR-1
>
CALLI 400024
>
IFN USER,<
JRST @STRADR-1
>
SETSET: MOVEI I,5
TCHK: MOVE J,TP(I)
JUMPE J,ATEND
SETZM TJ(I)
SETZM TN(I)
GNN: LDB J,[POINT 9,(J),17]
ADDI J,(CBUF)
CAIN J,(CBUF)
JRST[ MOVEM J,TP(I)
JRST ATEND]
JRST GNN
ATEND: SOJGE I,TCHK
PUSHJ P,EVAL
MOVEI I,5
FDTH: MOVE AC,TH(I)
MOVEM AC,T0(I)
FSBR AC,TFF(I)
MOVEM AC,DTH(I)
SETZM DELTH(I)
SETZM TDP(I)
SETZM TDD(I)
SOJGE I,FDTH
POPJ P,
UPDC: MOVE TAC,[XWD NC,PC]
BLT TAC,PCC+5
HRL AC,AC
HRRI AC,NC
BLT AC,NC+6
MOVE DATWD,NC+6
TLNE DATWD,NXTFN
JRST[ AOS AC,STKPTR
HLRE AC,(AC)
MOVEM AC,FUNCT
JRST .+1]
HRRZM DATWD,FBI
MOVEI K,5
UPL: MOVE AC,NC(K)
HLLZM AC,NC(K)
HRLZM AC,NCC(K)
MOVE AC,MOTARM(K)
FADRB AC,NCC(K)
TLNN DATWD,INT
JRST UPL1
FSBR AC,PCC(K)
MOVEM AC,DCII(K)
MOVE AC,NC(K)
FSBR AC,PC(K)
MOVEM AC,DCI(K)
UPL1: TDNE DATWD,BMASK(K)
TDO DATWD,BMASK(K)
SOJGE K,UPL
TLNE DATWD,INT
POPJ P,
MOVE AC,[XWD NC,CI]
BLT AC,CII+5
POPJ P,
PREAD:
IFN TRACK,<
SKIPE WALK
JRST[GNPR:AOBJP DATA,[ MOVE AC,[IOWD DBL,DATO]
MOVEM AC,DATPT
MOVE DATA,DATPT
SETZ TAC,
INPUT 15,1
JRST GNPR]
MOVE DACVAL,(DATA)
JRST @READFN]
>
MOVEI TAC,10
MOVEM TAC,DATMSD
DACRED: SOSGE DATMSD
JRST[REDER: MOVEI TAC,READER
MOVEM TAC,TRAJER
JRST TOFF]
DACST: CONO DB,4250
CONO AD,(K)
NREAD: MOVEI TAC,30
CONSO DB,1000
SOJGE TAC,.-1
JUMPL TAC,[DACDIE:CONO AD,4000(K) ;STOP DAC
TRO REREAD
IFN DEB,<MOVEM DACVAL,DACERR>
MOVEI TAC,12
WRH: SOJGE TAC,WRH
JRST DACRED]
DATAI DB,DACVAL
CONSZ DB,10000
JRST DACDIE
XOR DACVAL,SBMSK
IFN TRACK,<PUSH DATA,DACVAL>
JRST @READFN
FREAD: ADDI K,770000
IFN TRACK,<SKIPN WALK>
CONO AD,4000(K) ;STOP AD ON NEXT CHANNEL
POPJ P,
READFN: FREAD
XLIST
SUBTTL CONSTANTS LOU PAUL
LIST
NON: POINT 12,INNER(AC),11
POINT 12,INNER(AC),23
POINT 12,INNER(AC),35
POINT 12,OUTER(AC),11
POINT 12,OUTER(AC),23
POINT 12,OUTER(AC),35
ERR: 0.05
0.05
0.01
0.1
0.1
0.7
IFN SIMU,<
TH-1
ARM.TH: .+4
>
TH: -180.0 ;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
-90.0
12.0
-90.0
90.0
0.0
IFN SIMU,<ARM.GR:>
HAND: 3.0
TDD: BLOCK 6
ERRINT: 0
0
0
0
0
0
DTH: BLOCK 6
TD: BLOCK 6
T0P: BLOCK 6
TDP: BLOCK 6
ET0: BLOCK 6
T0: -150.0
-75.0
14.0
-90.0
60.0
0.0000
TFF: -150.0
-75.0
14.0
-90.0
60.0
0.0000
DELTH: BLOCK 6 ;THIS AND THE NEXT ARE BLT ED TOGETHER
DDTH: 0
0
0
0
0
0
NTICKS: 0
DTHS: BLOCK =30
TORE: BLOCK 6
T: BLOCK 6
IFN REPORT,<
PBLK: <SIXBIT/THETA/>+6
JER: BLOCK 6
<SIXBIT/DAC/>+6
>
IFN USER+REPORT+TRACK+DEB,<MOTOR: BLOCK 6>
FBI: 252502
VZ: BLOCK 3
IFN TACH,<
VDTH: BLOCK 3
>
THP: BLOCK 6
STQ: BLOCK 6
SDTQ: 0
ASTP: 0
ASTPP: 0
PC: BLOCK 6
PCC: BLOCK 6
DCI: BLOCK 6
DCII: BLOCK 6
CI: 0
0
0
0
0
0
CII: 1000.0
1200.0
100.0
100.0
100.0
100.0
NC: 0
0
0
0
0
0
NCC: 1000.0
1200.0
100.0
100.0
100.0
100.0
IFN DEB,<DACERR: 0>
REFTR: 0
ETIME: =16667
TDF: 1.0
IFN DEB,<
ETIM: 0
ACSAV: BLOCK 20
>
PDL: DHO
BLOCK PDLL
LTIME: 0
IFN SIMU,<ARM.FU:>
FUNCT: 0
ROT6: 0
TRAJER: 0
DOING: 0
DATMSD: 0
CBUFS: 0
DATWDS: 0
SNUM: POINT 12,DACVAL,23
SBMSK: 400040004000
REV: 0
0
-1
0
-1
-1
DMASK: 400000
100000
20000
4000
1000
200
BMASK: XWD 400000,200000
XWD 200000, 40000
XWD 100000, 10000
XWD 40000, 2000
XWD 20000, 400
XWD 10000, 100
GMASK: 200000
40000
10000
2000
400
100
VCHAN: 52B23
53B23
54B23
CHAN: 55B23
56B23
57B23
60B23
61B23
GAIN: MOVE 1,[SIXBIT/[ARM]/]
CALLI 1,400002
POPJ P,
THIRD: 0.3
SIXTH: 0.1
BLOCK =20
0
WAITIN: XWD 3,0
JUMPBK: XWD JMPABS,WAITIN
STKPTR: WAITIN
ERRSTK: 0
BITSAV: 0
COUNT: 0
STWT: 0
HCNT: 0
TP: 0
0
0
0
0
0
TJ: BLOCK 6
LOOP: BLOCK 10
TN: BLOCK 6
SC: BLOCK 6
A0: BLOCK 6
A1: BLOCK 6
A2: BLOCK 6
A3: BLOCK 6
A4: BLOCK 6
BLOCKS: 0
NXTBUF: 0
BUF: 0
BLOCK 1000
BUF2: 0
BLOCK 1000
IFN REPORT+TRACK,<
DATIWD: 0
0
DATPT: 0
DATO: BLOCK DBL+100
>
IFN DYNAMICS,<
DDAT: <SIXBIT/FORD/>+6
FOR: BLOCK 6
<SIXBIT/BACK/>+6
BAK: BLOCK 6
>